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摘要 : 针对 载体 的 姿态 和 角速度 估计 问题 ， 分 别 给 出 了 基于 卡尔 受 滤 波 和 粒子 滤波 的 估 
计算 法 。 对 于 欧 拉 角 带 来 的 奇异 问题 ， 采 用 四 元 数 描述 载体 的 姿态 角 。 通 过 对 mpu9250 模 
块 (陀螺 仪 、 加 速度 计 和 磁力 计 ) 进行 数据 采集 ， 并 运用 实验 数据 和 仿真 运算 ， 验 证 了 四 元 
数 卡 尔 曼 滤波 和 粒子 滤波 算法 的 可 行 性 。 静 态 实验 和 动态 仿真 的 计算 结果 表明 ， 在 使 用 微机 
械 惯 性 测量 器 件 测量 载体 姿态 时 ， 粒 子 滤波 和 卡尔 曼 滤 波 算法 得 到 的 误差 均值 相当 ， 但 粒子 
滤波 的 标准 差 相 对 较 小 。 
关键 词 ， 四 元 数 ; 卡尔 曼 滤波 ; 粒子 滤波 ; 姿态 估计 
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随 着 微小 型 飞行 器 和 机 器 人 等 的 快速 发 展 ， 对 其 姿态 研究 显得 越 来 越 重要 。 一 般 载体 的 姿态 常常 
采用 滤波 髓 对 来 自 微 机 械 (Micro-Electro-Mechanical System, MEMS ) 惯性 测量 单元 (IMU ) 的 多 传感器 数 
据 进 行 融合 得 到 ， 常 用 的 微机 械 惯 性 测量 器 件 包 括 三 轴 陀 螺 仪 、 三 轴 加 速度 计 和 三 轴 磁 力 计 。 文 [1 ] 
建立 了 四 元 数 卡尔 曼 滤波 方程 并 进行 了 数据 仿真 ， 文 [2] 对 文 [1] 提 出 的 四 元 数 卡尔 曼 滤波 进行 性 能 
分 析 。 同 时 很 多 人 也 进行 了 粒子 滤波 在 姿态 估计 中 的 研究 。 文 [4] 和 文 [5] 通 过 对 四 元 数 的 分 析 ， 
设计 了 四 元 数 粒 子 滤波 算法 求 取 姿 态 。 本 文通 过 对 微机 械 器 件数 据 采 集 和 处 理 ， 设 计 非 线性 四 元 数 卡 
尔 曼 滤波 和 粒子 滤波 ， 并 对 其 姿态 解 算 结果 进行 进一步 分 析 。 


1 ”四 元 数 简 介 


载体 上 的 微机 械 惯 性 测量 器 件 的 输出 转换 成 载体 的 姿态 ， 即 载体 自身 坐标 系 (x,, y,, z,) 相 对 于 
导航 坐标 系 (x,, Yno z ) 的 角 位 置 ， 写 成 矩阵 形式 如 下 : 


á X, X 
Y, 一 T Yn 3 ( 1) 
Zp g 


欧 拉 角 是 载体 的 3 个 姿态 角 ， 即 俯仰 角 (Piten ) ,— £878 ff ( Roll) 和 偏 航 角 ( Yaw) 。 根 据 欧 拉 旋转 定 
律 ， 可 以 通过 3 次 旋转 使 得 载体 坐标 系 与 导航 坐标 系 重 合 ， 每 一 次 旋转 都 绕 导 航 坐标 系 的 一 个 坐标 
轴 转 劲 ， 转 过 的 角度 就 是 欧 拉 角 ， 每 次 旋转 后 坐标 关系 可 由 一 个 旋转 矩阵 表示 ， 即 方向 余 弱 和 矩阵 ， 
(1) 式 采用 z-y-x 的 旋转 顺序 依次 旋转 : 

A, (6, 0, e) - AK o) ACO) A) = 
cos0cosQ cosOsingQ — sin 
sinjsinÜcosp — cosjsinp — sinósinÜsinp + cosÓcosp — sin$cosÓ | , (2) 


cosisinÜcosp + singsing — cos$sinÜsinp — sinjcosp — cosicos0 
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EF, y, o, 0 分 别 代表 偏 航 角 、 横 滚 角 、 俯 仰角 。 为 了 避免 欧 拉 角 在 表示 姿态 时 可 能 出 现 的 奇异 问 
题 ， 四 元 数 在 载体 的 姿态 表示 方面 有 广泛 的 应 用 。 设 计 载 体 姿 态 四 元 数 为 

gq = (go, qi; 4$, 43) 49 + dii + qj + qb, (3) 
导航 坐标 系 与 载体 坐标 系 之 间 的 坐标 变换 可 以 用 方向 余弦 抢 阵 表示 ， 其 四 元 数 形 式 为 


qu4émeg-u 2 fy) 2(09 4945) 
Alq) =| qgan) wow +g -gq XU +qq) |， (4) 
20g. eq). 2 -409) qq-q -q+g 


简 记 为 : 
E. d Ta 
A(g) =| Ta To als (5) 
fo Ta Ta 


导航 坐标 系 到 载体 坐标 系 的 旋转 变换 过 程 中 坐标 系 始终 保持 直角 坐标 系 ， 所 以 4(9q)， 为 正 交 坐 标 系 ， 
HU A(q); = AC) = [4(g);] 。 从 而 载体 的 姿态 角 为 


0 =- arcsin T 


723 
中 = arctan — 


Ts. (6) 


12 
Q — arctan — 
11 


四 元 数 的 微分 方程 如 下 
pis (7) 
其 中 ，g 为 描述 载体 转动 的 四 元 数 ，w 为 载体 相对 导航 坐标 系 的 角速度 ， 也 表示 为 四 元 数 的 形式 : 
o-0-t*oitojtok, (8) 
| do 0 -o, -o, -o0.||q, 
a w, 0 0, -o,||q; 
MEE | (9) 
: w w, -o, 0 q3 
IE : 
;= 二 (10) 
2-5 Pt. 


通过 三 轴 陀 螺 仪 测量 3 个 轴 的 角速度 就 可 以 实现 实时 更 新 四 元 数 的 值 ， 进 而 更 新 姿态 角 获 得 姿态 
2 ”四 元 数 卡 尔 曼 滤波 


在 四 元 数 和 非 线 性 滤波 算法 的 结合 中 ， 最 常用 的 算法 就 是 基于 四 元 数 卡 尔 曼 滤波 ， 本 文 使 用 文 
[1] 的 四 元 数 卡 尔 曙 滤波 ， 滤波 过 程 简介 如 下 : 

(1) 滤 波 器 初始 化 

H Q 和 民选 取 初 始 值 ， 可 以 通过 对 静态 下 三 轴 陀 螺 仪 、 三 轴 加 速度 计 和 三 轴 磁 力 计 进 行 测量 ， 
计算 出 各 个 轴 的 方差 作为 滤波 顺 数据 误差 ; 通过 初始 加 速度 计 测 量 出 俯仰 角 和 横 滚 角 ， 用 磁力 计 测量 
出 方位 角 作 为 初始 姿态 ， 并 把 初始 姿态 角 转 变 为 初始 四 元 数 qos， 选 择 Pu = 五 作为 系统 初始 噪声 。 

(2) 状态 方程 传播 


e 
> 
IF 
c 
o 


c— 
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文 [1] 中 的 Hamilton 算 子 定义 如 下 : 
E $ x v” 
H(q)- 11 
(q) | TS " : (11) 
HEP, s 表示 四 元 数 的 标量 ; v 表示 四 元 数 的 矢量 。 


A = Ho) ， (12) 

Dun = op [5 018. (13) 

Qa = a dia > (14) 

Ma = qi qu + Ps (15) 

Phan = Bina Pla Ba + Dra) I -Mal Q (16) 


(12) 式 到 (16) 式 中 ，gij 为 时 刻 将 矢量 在 RR 系 中 的 投影 转 到 B 系 中 的 姿态 四 元 数 ，owi 为 6 时刻 将 
B 系 相对 于 R 系 的 角速度 在 B 系 中 的 投影 ，B,i 为 姿态 转移 矩阵 ; gu 和 PLU T MNA ASTE qu, 
的 递 推 估计 值 和 协 方差 矩阵 。 

(3) 量 测 方程 更 新 

首先 通过 预测 得 到 的 quiu, 应 用 (4) 式 计算 出 观测 方程 中 的 系数 矩阵 HC quai) - 


Mya = dian den + Phan (17) 

Bor = Müa): (18) 

Pis =F Rin raa) L ~ Mina 7 Bia Mania Bla , (19) 
3a S Hu Eu HOD a (20) 
lont (21) 

diia 7 i * Ka Doa 7 HG) mal (22) 
Plana = U ~ Kon Egona) Phan (23) 


在 (18) 式 到 (23) 式 中 ，5b,,1 为 空间 矢量 在 B 系 中 的 投影 四 元 数 ，Pi,iy 为 计算 出 来 的 状态 协 方差 ，K,， 
为 ti 时刻 的 卡尔 曼 增 益 系 数 ; gui 为 二 时 刻 的 递 推 四 元 数 佑 计 ; Ps 为 四 元 数 卡 尔 曼 滤波 方程 
的 观测 协 方差 。 


3 ”粒子 滤波 


粒子 滤波 是 通过 寻找 一 组 在 状态 空间 中 传播 的 随机 样本 近似 表示 状态 变量 ， 用 样本 均值 代替 积 4 
运算 ， 进 而 获得 系统 最 小 方差 的 过 程 ， 这 些 样 本 被 称 为 粒子 。 采 用 数学 语言 描述 如 下 : 对 于 平稳 的 随机 
过 程 ， 假 设 k-1 时 刻 系统 的 后 验 概率 为 p(x,-1|z,-1) ， 依 据 一 定 原则 选取 nn 个 随机 样本 ，% 时 刻 获得 测 
量 更 新 后 ， 经 过 状态 和 时 间 过 程 ，n 个 粒子 的 后 验 概 率 密度 可 近似 为 p(wi |z,)， 随 着 粒子 数目 的 增加 ， 
粒子 的 概率 密度 函数 逐渐 允 近 状态 的 概率 密度 函数 ， 从 而 粒子 滤波 达到 最 优 贝 叶 斯 估计 的 效果 "| 。 

假设 非 线 性 动态 离散 系统 为 


Xr mf. wi) (24) 


zh, Di) 


其 中 , x, e R" 为 时 刻 的 n 维 状 态 向 量 ;ze R” 2 m 维 观测 向 量 ; wi 和 vw 分 别 为 过 程 噪声 和 量 测 噪声 。 
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粒子 滤波 算法 步骤 如 下 ; 
(1) 初 始 化 : = 0 时， 产生 服从 初始 概率 密度 plr) 的 个 粒子 点 xD ，i = len; 
(2) 通 过 重要 性 采样 更 新 样本 粒子 状态 ， 
n d q(x, leas Zi, a) , (25) 
E = la Dy SIN, (26) 
(3) 计算 更 新 后 粒子 的 权 值 


y PG Ix? ) p? In ) 


w? = wÈ "nu Ix. dc E (27) 
(4) 归 一 化 粒子 权 值 
wf? sar /> w. (28) 
(5) 重 采样 :根据 各 自 归 一 化 权 值 w 的 大 小 复制 /舍弃 样本 ， 得 到 NN 个 近似 服从 p aE ) 1a) 
分 布 的 样本 xf ， 令 wp L, im ee 


(6) 输 出 结果 : 算法 的 输出 是 粒子 集 fx 和 4 ,i =1…N} ， 用 它 可 以 表示 后 验 概率 和 函数 g(xo, 1) 


的 期 望 : TA 
p(xo, 12, 4) anA Sx’, (dxo. i), (29) 


Flats, 017 E eo. (30) 
(7) 令 不 = 大 +1， 重 复 以 上 步骤 。 


4 数据 实验 


实验 通过 选择 传感器 模块 mpu9250， 它 包含 三 轴 陀 螺 仪 、 三 轴 加 速度 计 和 三 轴 磁 力 计 ， 能 够 通过 
自身 所 有 的 低 通 滤波 器 和 A/D 变换 模块 直接 输出 九 轴 传 感 器 数据 。 实 验 通过 将 mpu6050 固定 在 转台 
上 测量 ， 在 安装 过 程 中 远离 环境 磁场 的 干扰 。 采 用 单片机 读 取 mpu9250 测量 数据 后 通过 20 串口 传 
输 给 电脑 进行 处 理 ， 以 检验 算法 的 可 用 性 和 精度 。 

(1) 首 先 将 传感器 模块 mpu9250 在 静止 状态 下 测量 输出 数据 ， 采 样 速率 为 100 Hz, 采样 1 min, 
分 析 静 态 情 况 下 的 九 轴 输出 数据 。 陀 螺 仪 三 轴 静 态 数据 如 图 1。 


Ka 0.03 
m 0.02 
PI 
59 
0.01 
0 10 20 30 40 50 60 
时 间 /s 
Ka 0 
Ed -0.005 T | | 
& -0.01 
0 10 20 30 40 50 60 
时 间 /s 


角 速 率 /(?"/S) 
E E 
一 [o — 


时 间 /s 
图 1 陀螺 仪 三 轴 静 态 输 出 数据 


Fig.1  Three-axis gyroscope static output data 
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图 1 是 1 min 采集 的 陀螺 仪 三 轴 数 据 ， 自 上 而 下 依次 是 轴 、y 轴 和 =z 轴 ， 运 用 MATLAB 软件 分 


Vr x 轴 的 均值 和 方差 分 别 为 0. 0205(°)/s 4$11.238e( 74) (?)/s; y 轴 的 均值 和 方差 分 别 为 ， -0. 004 8 
(?)/s PI 1.57e(-4) (?)/s; z 轴 的 均值 和 方差 分 别 为 : 0.015 4(°*)/s 和 2.369 9e(-4)(?")《s。 用 同样 
的 方法 处 理 三 轴 加 速度 计 和 三 轴 陀 螺 仪 的 原始 数据 ， 结 果 见 表 1。 


通过 以 上 测量 ， 加 速度 计 在 水 平 位 置 x 轴 和 Yy 轴 均 值 不 为 0， 所 以 在 做 姿态 解 算 时 引入 均值 误差 


提高 测量 精度 。 


(2) 通 过 上 面 静 态 数据 的 读 取 ， 将 各 个 传感器 的 方差 作为 四 元 数 卡尔 曼 滤 波 和 粒子 滤波 程序 的 方 


差 参 考 ， 静 止 状态 下 四 元 数 卡尔 曼 滤 波 和 粒子 滤波 的 姿态 角 结算 误差 如 图 2、 图 3。 其 中 横 坐 标 表示 
时 间 ， 单 位 是 秒 ; 纵 坐 标 表示 输出 的 角度 误差 ， 单 位 是 (°) 


表 1 加 速度 计 和 陀螺 仪 输出 数据 的 均值 和 方差 


Table 1 Mean and variance of accelerometer and gyro output data 


三 轴 加 速度 计 /g 三 轴 磁 力 计 /uT 
x ll y 轴 EE x 轴 y 轴 z 轴 
均值 0.4 -0.5 9.4 3.6 4.5 2.1 
方差 0.0027 0. 003 3 0. 003 1 0. 02 0. 096 0. 01 


-0.5 
0 10 20 30 40 50 60 
0.4 
PN 
0.4 
0 10 20 30 40 50 60 
0.4 


0.2 
of Ws 
-0.2 

40 50 60 


0 10 20 30 
图 2 四 元 数 卡 尔 曼 滤波 静态 三 轴 姿 态 角 误差 


Fig.2  Quaternion Kalman filter static triaxial attitude angle error 


0 10 20 30 40 50 60 


图 3 粒子 滤波 静态 三 轴 姿 态 角 误 差 


Fig.3 Particle filter static triaxial attitude angle error 
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静止 状态 下 四 元 数 卡尔 曼 滤波 和 粒子 滤波 三 轴 姿 态 角 均值 和 误差 如 表 2。 通 过 对 静态 数据 的 分 析 
可 以 看 出 ， 两 种 算法 对 均值 改善 相差 不 多 ， 但 是 粒子 滤波 比 四 元 数 卡 尔 曼 滤波 在 方差 上 有 所 改善 ， 即 
粒子 渡 波 静态 测量 相对 稳定 。 

(3) 由 于 没有 转台 等 实验 设备 ， 通 过 对 静态 数据 添加 一 个 稳定 的 姿态 仿真 轨迹 计算 四 元 数 卡尔 曼 
滤波 和 粒子 滤波 在 动态 情况 下 的 结算 结果 。 通 过 对 三 轴 姿 态 角 分 别 添加 角 速 率 为 2，3，5(*)/s， 幅 
度 为 5 仿真 ， 得 到 如 图 4 的 仿真 结果 。 其 中 横 坐 标 表示 时 间 ， 单 位 是 秒 ， 纵 坐标 表示 输出 的 角度 误 


差 ， 单位 是 (°) 


表 2 静止 状态 两 种 算法 的 均值 和 方差 


Table 2 Mean and variance of the two algorithms for the quiescent state 


四 元 数 卡尔 曼 粒子 滤波 
Xx 轴 y 轴 Z 轴 Xx E y 轴 zZ 轴 
均值 0. 002 0. 003 6 0. 002 7 —0. 033 7 0. 036 9 0. 001 6 
方差 0.0106 0.0087 0.0101 0.0143 0.0024 0.0045 
0.5 
| | TAA PSU " ] E NY A 人 JAVA 
Mtl UA Au tit uu 
-0.5 1 1 1 1 1 j 
0 10 20 30 40 50 60 
0.5 
0 UT M CUAL PW Hia AN Nl M ENIM) TCI VM "wi | | aod MM VIAL 
-0.5 L L 1 —1 L 1 
0 10 20 30 40 50 60 
0.5 
0 li RUM. Pl y P WE TT Mon a nC ea PN NUN VN CT NT ^ UAI NM MANN A TAT 
-0.5 
0 10 20 30 40 50 60 
图 4 两 种 算法 姿态 角 误差 比较 
Fig.4 Comparison of Attitude Angle Errors of Two Algorithms 
表 3 仿真 状态 两 种 算法 的 均值 和 方差 
Table 3 The mean and variance of the two algorithms for Simulation states 
四 元 数 卡 尔 曼 粒子 滤波 
Xx 轴 y 轴 Z 轴 Xx E y 轴 zZ 轴 
均值 0. 030 1 0. 020 1 0. 025 4 —0. 034 7 0.018 0 0. 020 4 
方差 0.014 0.0095 0.0134 0.0161 0.0050 0.0085 


从 表 3 可 以 看 出 ， 通 过 对 动态 的 仿真 实验 分 析 ， 两 种 算法 在 均值 改善 和 静态 数据 基本 相同 ,但 是 
粒子 滤波 比 四 元 数 卡尔 曼 滤 波 在 方差 上 有 所 改善 ， 即 动态 情况 下 粒子 滤波 相对 稳定 。 


5 结 论 


本 文通 过 对 四 元 数 卡 尔 曼 滤波 和 粒子 滤波 进行 简单 介绍 ， 并 通过 对 mpu9250 进行 数据 采集 和 仿 
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真实 验 ， 验 证 粒子 滤波 和 四 元 数 卡尔 曼 滤 波 的 可 行 性 ， 以 误差 均值 和 标准 差 为 检验 标准 ， 验 证 了 粒子 
滤波 相对 四 元 数 卡尔 曼 滤 波 提 高 了 标准 差 。 
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Abstract: Two algorithms are presented to solve the carrier attitude and rate estimation problem. One is 
based on Kalman filter and the other is based on Particle filter. In order to avoid the singular problem from 
Euler angle, we chose quaternion to describe the attitude angle. We carried out the data acquisition. of 
gyroscope, accelerometer and magnetometer of mpu9250 module. By comparing the experimental data and 
simulation, we discussed the feasibility of quaternion Kalman filter and particle filter algorithm. The results of 
static and dynamic simulation show that the errors of the particle filter and the Kalman filter are similar when 
the carrier attitude is measured by the MEMS device, and the standard deviation of the particle filter is 
relatively small. 
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